import numpy as np
import cv2
from socket import *
import mediapipe as mp
import time
import math
    
def vector_2d_angle(v1,v2):
    '''
        求解二维向量的角度
    '''
    v1_x=v1[0]
    v1_y=v1[1]
    v2_x=v2[0]
    v2_y=v2[1]
    try:
        angle_= math.degrees(math.acos((v1_x*v2_x+v1_y*v2_y)/(((v1_x**2+v1_y**2)**0.5)*((v2_x**2+v2_y**2)**0.5))))
    except:
        angle_ =65535.
    if angle_ > 180.:
        angle_ = 65535.
    return angle_

# 测两点距离
def cal_distance(pos1, pos2):
    return math.sqrt((pos1[0] - pos2[0])**2 + (pos1[1] - pos2[1]) ** 2)

# v: [pos1, pos2] [3, 4]
def hand_angle(hand_, v1, v2):
    '''
        获取对应手相关向量的二维角度,根据角度确定手势
    '''
    angle = vector_2d_angle(
        ((int(hand_[v1[0]][0])- int(hand_[v1[1]][0])),(int(hand_[v1[0]][1])-int(hand_[v1[1]][1]))),
        ((int(hand_[v2[0]][0])- int(hand_[v2[1]][0])),(int(hand_[v2[0]][1])- int(hand_[v2[1]][1])))
        )

    return angle

# 检测 0 || ok
def gesture_0(hand_):
    
    # flag = hand_angle(hand_, [0, 1], [0, 5]) > 20
    # flag = flag and hand_angle(hand_, [5, 6], [6, 7]) > 60 
    # flag = flag and cal_distance(hand_[4], hand_[8]) < 0.8 * cal_distance(hand_[1], hand_[2])
    # flag = flag and cal_distance(hand_[4], hand_[20]) >  cal_distance(hand_[0], hand_[1])
    # flag = flag and cal_distance(hand_[4], hand_[12]) >  cal_distance(hand_[11], hand_[12])
    # if flag:
    #     return 0
    flag = hand_angle(hand_, [6, 5], [6, 7]) > 160
    #return hand_angle(hand_, [6, 5], [6, 7])
    flag = flag and hand_angle(hand_, [7, 6], [7, 8]) > 160
    # flag = flag and hand_angle(hand_, [10, 9], [10, 11]) < 150
    # flag = flag and hand_angle(hand_, [2, 1], [2, 3]) < 160
    # flag = flag and hand_angle(hand_, [18, 17], [18, 19]) < 150
    flag = flag and cal_distance(hand_[12], hand_[8]) >  cal_distance(hand_[8], hand_[5])
    flag = flag and cal_distance(hand_[4], hand_[8]) >  cal_distance(hand_[8], hand_[5])
    flag = flag and cal_distance(hand_[4], hand_[5]) <  cal_distance(hand_[7], hand_[5])
    if flag:
        return 1
    flag = hand_angle(hand_, [10, 9], [10, 11]) > 150
    flag = flag and hand_angle(hand_, [11, 10], [11, 12]) > 150
    flag = flag and hand_angle(hand_, [7, 6], [7, 8]) > 150
    flag = flag and hand_angle(hand_, [6, 5], [6, 7]) > 150
    # flag = flag and hand_angle(hand_, [14, 13], [14, 15]) < 120
    # flag = flag and hand_angle(hand_, [18, 17], [18, 19]) < 120
    # flag = flag and cal_distance(hand_[4], hand_[14]) <  cal_distance(hand_[5], hand_[6])
    flag = flag and cal_distance(hand_[9], hand_[10]) >  cal_distance(hand_[4], hand_[16])
    if flag:
        return 2
    
    flag = hand_angle(hand_, [10, 9], [10, 11]) > 150
    flag = flag and hand_angle(hand_, [11, 10], [11, 12]) > 150
    flag = flag and hand_angle(hand_, [7, 6], [7, 8]) > 150
    flag = flag and hand_angle(hand_, [6, 5], [6, 7]) > 150
    flag = flag and hand_angle(hand_, [14, 13], [14, 15]) > 150
    flag = flag and hand_angle(hand_, [15, 14], [15, 16]) > 150
    # flag = flag and hand_angle(hand_, [18, 17], [18, 19]) < 120
    flag = flag and cal_distance(hand_[4], hand_[20]) <  cal_distance(hand_[5], hand_[13])
    if flag:
        return 3
    
    flag = hand_angle(hand_, [10, 9], [10, 11]) > 150
    flag = flag and hand_angle(hand_, [11, 10], [11, 12]) > 150
    flag = flag and hand_angle(hand_, [7, 6], [7, 8]) > 150
    flag = flag and hand_angle(hand_, [6, 5], [6, 7]) > 150
    flag = flag and hand_angle(hand_, [14, 13], [14, 15]) > 150
    flag = flag and hand_angle(hand_, [15, 14], [15, 16]) > 150
    flag = flag and hand_angle(hand_, [18, 17], [18, 19]) > 150
    flag = flag and hand_angle(hand_, [19, 18], [19, 20]) > 150
    flag = flag and cal_distance(hand_[4], hand_[17]) <  cal_distance(hand_[0], hand_[17])
    if flag:
        return 4
    
    flag = hand_angle(hand_, [10, 9], [10, 11]) > 150
    flag = flag and hand_angle(hand_, [11, 10], [11, 12]) > 150
    flag = flag and hand_angle(hand_, [7, 6], [7, 8]) > 150
    flag = flag and hand_angle(hand_, [6, 5], [6, 7]) > 150
    flag = flag and hand_angle(hand_, [14, 13], [14, 15]) > 150
    flag = flag and hand_angle(hand_, [15, 14], [15, 16]) > 150
    flag = flag and hand_angle(hand_, [18, 17], [18, 19]) > 150
    flag = flag and hand_angle(hand_, [19, 18], [19, 20]) > 150
    flag = flag and hand_angle(hand_, [3, 2], [3, 4]) > 150
    
    if flag:
        return 5
    
    flag = hand_angle(hand_, [3, 2], [3, 4]) > 150
    flag = flag and hand_angle(hand_, [18, 17], [18, 19]) > 150
    flag = flag and hand_angle(hand_, [19, 18], [19, 20]) > 150
    flag = flag and cal_distance(hand_[4], hand_[20]) >  cal_distance(hand_[8], hand_[0])
    if flag:
        return 6
    
    flag = cal_distance(hand_[4], hand_[8]) <  cal_distance(hand_[0], hand_[17])
    flag = flag and cal_distance(hand_[4], hand_[12]) <  cal_distance(hand_[0], hand_[17])
    flag = flag and cal_distance(hand_[4], hand_[16]) <  cal_distance(hand_[0], hand_[17])
    flag = flag and cal_distance(hand_[4], hand_[20]) <  cal_distance(hand_[0], hand_[17])
    flag = flag and cal_distance(hand_[8], hand_[12]) <  cal_distance(hand_[0], hand_[17])
    flag = flag and cal_distance(hand_[0], hand_[12]) >  cal_distance(hand_[0], hand_[1])
    flag = flag and cal_distance(hand_[4], hand_[8]) <  cal_distance(hand_[2], hand_[3])
    flag = flag and cal_distance(hand_[8], hand_[20]) <  cal_distance(hand_[2], hand_[3])
    if flag:
        return 7
    
    flag = cal_distance(hand_[4], hand_[8]) >  cal_distance(hand_[5], hand_[8])
    flag = flag and cal_distance(hand_[0], hand_[12]) <  cal_distance(hand_[0], hand_[9])
    flag = flag and cal_distance(hand_[4], hand_[18]) >  cal_distance(hand_[17], hand_[5])
    if flag:
        return 8
    
    flag = cal_distance(hand_[5], hand_[8]) <  1.1 * cal_distance(hand_[5], hand_[7])
    flag = flag and cal_distance(hand_[0], hand_[12]) <  cal_distance(hand_[0], hand_[8])
    flag = flag and cal_distance(hand_[8], hand_[0]) >  cal_distance(hand_[0], hand_[12])
    flag = flag and cal_distance(hand_[6], hand_[10]) >  cal_distance(hand_[6], hand_[7])
    flag = flag and cal_distance(hand_[6], hand_[10]) >  cal_distance(hand_[9], hand_[5])
    flag = flag and hand_angle(hand_, [5, 0], [5, 6]) > 150
    if flag:
        return 9
    
    flag = cal_distance(hand_[5], hand_[8]) <  1.1 * cal_distance(hand_[5], hand_[7])
    flag = flag and cal_distance(hand_[0], hand_[12]) <  cal_distance(hand_[0], hand_[8])
    flag = flag and cal_distance(hand_[8], hand_[0]) <  cal_distance(hand_[0], hand_[5])
    if flag:
        return 10
    
    return -1


def h_gesture(angle_list):

    thr_angle = 65.  #手指闭合则大于这个值（大拇指除外）
    thr_angle_thumb = 53.  #大拇指闭合则大于这个值
    thr_angle_s = 49.  #手指张开则小于这个值
    gesture_str = "Unknown"

        
    return gesture_str

def start():
    cap = cv2.VideoCapture(0)
    mpHands = mp.solutions.hands
    hands = mpHands.Hands(min_detection_confidence=0.5)
    mpDraw = mp.solutions.drawing_utils
    handLmsStyle = mpDraw.DrawingSpec(color=(0, 0, 255), thickness=5)
    handConsStyle = mpDraw.DrawingSpec(color=(0, 255, 0), thickness=10)
    pTime = 0
    cTime = 0
    
    
    # tcp_client_socket = socket(AF_INET, SOCK_STREAM)
    # tcp_client_socket.connect(("3.3.3.3", 12345))
    # tcp_client_socket.send(str(123).encode("utf-8"))
    
    
    # # 接收数据
    # recv_data = tcp_client_socket.recv(1024).decode("gb2312")
    # print(recv_data)
    
    # # 关闭套接字
    # tcp_client_socket.close()
    
    
    # 使用IPv4 UDP方式来创建套接字
    udp_socket = socket(AF_INET,SOCK_DGRAM)
    
    #2.绑定一个本地信息
    localaddr = ("0.0.0.0",8080)
    udp_socket.bind(localaddr)

    
    # 设置允许发送广播
    udp_socket.setsockopt(SOL_SOCKET, SO_BROADCAST, True)
    



    

    s = socket(AF_INET, SOCK_DGRAM) # 创建UDP套接字
    addr = ('0.0.0.0', 12345)  # 0.0.0.0表示本机
    s.bind(addr)

    s.setblocking(0) # 设置为非阻塞模式
    num = 0
    cur_pos = []
    while True:
        data = None
        try:
            data, _ = s.recvfrom(9216000)
            receive_data = np.frombuffer(data, dtype='uint8')
            img = cv2.imdecode(receive_data, 1)

            cv2.putText(img, "server", (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2)

    #
            imgRGB = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)
            result = hands.process(imgRGB)
            # print(result.multi_hand_landmarks)
            imgHeight = img.shape[0]
            imgWidth = img.shape[1]

            if result.multi_hand_landmarks:
                for handLms in result.multi_hand_landmarks:
                    hand_pos_collection = []
                    mpDraw.draw_landmarks(img, handLms, mpHands.HAND_CONNECTIONS, handLmsStyle, handConsStyle)
                    for i, lm in enumerate(handLms.landmark):
                        xPos = int(lm.x * imgWidth)
                        yPos = int(lm.y * imgHeight)
                        zPos = int(lm.z )
                        # cv2.putText(img, str(i), (xPos - 25, yPos + 5), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 0, 255), 2)
                        hand_pos_collection.append((xPos, yPos))
                        # if i == 4:
                        #     cv2.circle(img, (xPos, yPos), 20, (0, 0, 255), cv2.FILLED)

                    if hand_pos_collection:
                #     # angle_list = hand_angle(hand_pos_collection)
                        # print(gesture_0(hand_pos_collection))
                        
                        cur_pos.append(gesture_0(hand_pos_collection))
                        num = num + 1
                        if len(cur_pos) == 3:
                            pos_ = cur_pos[0]
                            flag = 1
                            for pos in cur_pos:
                                if pos_ != pos:
                                    flag = 0
                            if flag == 1 and cur_pos[0] != -1:
                                    # 发送数据
                                    # 建立连接
                                    # 发送信息
                                udp_socket.sendto(str(cur_pos[0]).encode("gb2312"), ("255.255.255.255", 8080))    
                                print(cur_pos[0])
                            num = 0
                            cur_pos = []
                                
                                
                            
                        
            cTime = time.time()
            fps = 1/(cTime - pTime)
            pTime = cTime
            # cv2.putText(img, f"FPS : {int(fps)}", (30, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 3)
    #

            # cv2.imshow('server', img)
        except BlockingIOError as e:
            pass

        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

    cv2.destroyAllWindows()


if __name__ == '__main__':
    start()